Efficient Algorithms for Globally Optimal Trajectories

We present serial and parallel algorithms for solving a system of equations that arises from the discretization of the Hamilton-Jacobi equation associated to a trajectory optimization problem of the following type. A vehicle starts at a prespecified point zo and follows a unit speed trajectory ~ ( t ) inside a region in P. until an unspecified time T that the region is exited. A trajectory minimizing a cost function of the form T(T( t ) ) dt+ q ( c ( T ) ) is sought. The discretized Hamilton-Jacobi equation corresponding to this problem is usually solved using iterative methods. Nevertheless, assuming that the function P is positive, we are able to exploit the problem structure and develop onepass algorithms for the discretized problem. The first algorithm resembles Dijkstra’s shortest path algorithm and runs in time O(n log n ) , where n is the number of grid points. The second algorithm uses a somewhat different discretization and borrows some ideas from a variation of Dial’s shortest path algorithm that we develop here; it NIIS in time O ( n ) , which is the best possible, under some fairly mild assumptions. Finally, we show that the latter algorithm can be efficiently parallelized: for twodimensional problems and with p processors, its running time becomes O ( n / p ) , provided that p = O( fi/ log n) .